/**
 *****************************************************************************
 * @file
 * @brief
 * @author taoye
 * @version V0.0.1
 * @date 2021/08/24
 ****************************************************************************
 */
  /* Includes ------------------------------------------------------------------*/
#include <math.h>
#include "func_pid.h"
#include "string.h"

/* Private defines -----------------------------------------------------------*/

/* Private Struct  -----------------------------------------------------------*/


/* Private enum    -----------------------------------------------------------*/


/* Private Variable  ---------------------------------------------------------*/

/***********************************************************************************
/*变积分系数处理函数，实现一个输出0和1之间的分段线性函数                    */
/* 当偏差的绝对值小于最小值时，输出为1；当偏差的绝对值大于最大值时，输出为0   */
/* 当偏差的绝对值介于最大值和最小值之间时，输出在0和1之间现行变化             */
/* float error，当前输入的偏差值                                              */
/* float absmax，偏差绝对值的最大值                                           */
/* float absmin，偏差绝对值的最小值                                           */
/***********************************************************************************/
static float VariableIntegralCoefficient(float error,float absmax,float absmin)
{
  float factor=0.0;

  if(fabs(error)<=absmin)
  {
    factor=1.0;
  }
  else if(fabs(error)>absmax)
  {
    factor=0.0;
  }
  else
  {
    factor=(absmax-fabs(error))/(absmax-absmin);
  }

  return factor;
}

/***********************************************************************************
 * @brief PID位置式
 * ex:    
 * @par   
 * None   
 * @retval void None
 **********************************************************************************/
float PID_Positional(func_pid_t *this, float input)     
{   
    this->private.ActualVal = input;
    
    this->private.err = this->SetVal - this->private.ActualVal;     // 偏差等于目标值减实际值
    
    /*积分项，积分分离，偏差较大时去掉积分作用*/
	if( (fabs(this->private.err)/this->SetVal) <  this->integral_sep ){
		
		//this->private.integral += this->private.err;  
		
//		/*积分范围限定，防止积分饱和*/
//		if(this->private.integral >= this->integral_lim)         // 积分量上限
//			this->private.integral = this->integral_lim;
//    
//		if(this->private.integral <= -this->integral_lim)         // 积分量上限
//			this->private.integral = -this->integral_lim;
	}
    
	#if 1
//	if( (fabs(this->private.err)/this->SetVal) <  this->integral_sep){
//		this->private.outval = (0.2*this->Kp * this->private.err) +
//							   (0.2*this->Ki * this->private.integral) +
//							   (0.2*this->Kd * (this->private.err - this->private.err_last));    //输出值计算范围0~1
//	}else{
//		this->private.outval = (this->Kp * this->private.err) +
//                           (this->Ki * this->private.integral) +
//                           (this->Kd * (this->private.err - this->private.err_last));    //输出值计算范围0~1
//	}
	
	if( fabs(this->private.err) <  1000){
		this->private.outval = (0.95*this->Kp * this->private.err) +
							   (0.0*this->Ki * this->private.integral) +
							   (this->Kd * (this->private.err - this->private.err_last));    //输出值计算范围0~1
	}else if( fabs(this->private.err) <  500){
		this->private.outval = (0.5*this->Kp * this->private.err) +
                               (this->Ki * this->private.integral) +
                               (this->Kd * (this->private.err - this->private.err_last));    //输出值计算范围0~1
	}else {
		this->private.outval = (this->Kp * this->private.err) +
                               (this->Ki * this->private.integral) +
                               (this->Kd * (this->private.err - this->private.err_last));    //输出值计算范围0~1
	}
	
	#else
    this->private.outval = (this->Kp * this->private.err) +
                           (this->Ki * this->private.integral) +
                           (this->Kd * (this->private.err - this->private.err_last));    //输出值计算范围0~1
	#endif
    
    if(this->private.outval > 1)     this->private.outval = 1;      //输出范围限制
    if(this->private.outval < 0)     this->private.outval = 0;
    
    this->private.err_last = this->private.err;                       //更新偏差历史值
    
    return this->private.outval;
}

float K_mutil1 = 0.0001;
float K_integ1 = 0.001;
float K_mutil2 = 0.05;
float K_integ2 = 0.0001;
float K_mutil3 = 5;
float K_integ3 = 0.00001;
float K_mutil4 = 1;
float K_mutil5 = 2;
/***********************************************************************************
 * @brief  PID增量式
 * ex:    
 * @par   
 * None   
 * @retval void None
 **********************************************************************************/
float PID_Incremental(func_pid_t *this, float input, float input_press)     
{   
    static float init_value = 0;
	
	
	this->private.ActualVal = input;
    
    this->private.err = fabs(this->SetVal - this->private.ActualVal);     // 偏差等于目标值减实际值
    
	
	if(input_press < 5000)
	{
		this->private.outval = K_mutil5 * this->Kp * this->private.err + K_integ3 *this->Ki * this->private.err_last + this->Kd * this->private.err_last2;
	}
	else if( input_press < 15000)
	{
		this->private.outval = K_mutil4 * this->Kp * this->private.err + K_integ3 *this->Ki * this->private.err_last + this->Kd * this->private.err_last2;
	}
	else if( input_press <  30000)
	{
		this->private.outval = K_mutil3 * this->Kp * this->private.err + K_integ3 *this->Ki * this->private.err_last + this->Kd * this->private.err_last2;
	}
	else if( input_press <  45000)
	{
		this->private.outval = K_mutil2 * this->Kp * this->private.err + K_integ2 *this->Ki * this->private.err_last + this->Kd * this->private.err_last2;
	}
	else
	{
		this->private.outval = K_mutil1 * this->Kp * this->private.err + K_integ1 * this->Ki * this->private.err_last + this->Kd * this->private.err_last2;
	}

 
	this->private.err_last2 = this->private.err_last;
    this->private.err_last = this->private.err;

    if(this->private.outval > 1)     this->private.outval = 1;      //输出范围限制
    if(this->private.outval < 0)     this->private.outval = 0;
	
	return this->private.outval * this->outval_lim;
}


void PID_Clear(func_pid_t * this)
{
	memset(&this->private, 0, sizeof(this->private));
}

/***********************************************************************************
 * @brief
 * ex:    
 * @par   
 * None   
 * @retval void None
 **********************************************************************************/
int Func_Pid_Init( func_pid_t *this )
{
    this->private.ActualVal = 0;
    this->private.err       = 0;
    this->private.err_last  = 0;
	this->private.err_last2 = 0;
	this->private.errMax    = 0;
	this->private.errMin    = 0;
	this->private.deadArea  = 0;
    this->private.outval    = 0;
    this->private.integral  = 0;
	this->outval_lim = 1;
	

    this->Pos = PID_Positional;
	this->Inc = PID_Incremental;
	this->Clear = PID_Clear;

    return 0;
}

